#include "motion.h"
#include "x42.h"
#include <math.h>

// 全局变量保存当前位置
static float current_angle = 0.0f;
void set_motor_angle(uint8_t num, float target_angle)
{
	switch(num)
	{
		case 0x01:
		{
			// 计算角度差
			uint8_t direction;
			float angle_diff = fabs(target_angle - current_angle);
			// 更新当前位置
			current_angle = target_angle;
			if (target_angle > -1)
			 direction = 0;
			else 
			{
				direction = 1 ;
				target_angle =   fabs(target_angle) ;
			}
			// 转换为步进电机所需的脉冲数 (假设1.8度/步，16细分)
			uint32_t steps = (uint32_t)(target_angle / 1.8f * 16);
			Emm_V5_Pos_Control(0x01, direction, 10, 0, steps, 0x01, 0);
			break;
		}
			// 可以添加其他case处理更多电机
	}
}
